#include "hwMotor.h"
#include "tim.h"

static struct
{
    uint8 inited;
    uint8 startingCnt;
    int16 pwm;
    int16 lastPwm;
}rd;

#define setRdInited(x)       scnsBitSet(rd.inited,0,x)
#define isRdInited()         scnsBitGet(rd.inited,0)
#define setRdStarting(x)     scnsBitSet(rd.inited,1,x)
#define isRdStarting()       scnsBitGet(rd.inited,1)

void hwMotorInit(void)
{
    __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_1,0);
    __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_2,0);
    HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_1);
    HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_2);
    scnsMenuSetPage(8,"hw",NULL);
    scnsMenuAddStruct(8,rd,pwm,,SCNS_TYPE_INT16,1,0XFFFF,1000,10);
    setRdInited(1);
}

void hwMotorInterrupt(void)
{
    if(!isRdInited()){return;}
    rd.pwm=scnsLimit(rd.pwm,7000);
    if(scnsAbs(rd.pwm)<=900)
    {
        rd.pwm=0;
        setRdStarting(0);
        rd.startingCnt=0;
    }
    else if(((int32)rd.pwm)*((int32)rd.lastPwm)<=0)
    {
        setRdStarting(1);
    }
    rd.lastPwm=rd.pwm;

    int16 pwm=rd.pwm;
    if(isRdStarting())
    {
        pwm=pwm>=0?7000:-7000;
        ++rd.startingCnt;
        if(rd.startingCnt>=20){setRdStarting(0);}
    }
    else
    {
        rd.startingCnt=0;
    }
    if(pwm>=0)
    {
        __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_2,0);
        __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_1,pwm);
    }
    else
    {
        __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_1,0);
        __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_2,-pwm);
    }
}
